#include "tim.h"
#include "usart.h"
#include "general_task_include.h"
#include "CAN_driver.h"
#include "LK_driver.h"
//#include "judge_receive.h"
//#include "usb_device.h"
//#include "usbd_cdc_if.h"

//#include "WS2812.h"
//#include "UI_driver.h"

extern const Control* _control ;
/**
 * @brief 电机的CAN信号帧发送
 * @note  总线挂载情况：CAN1-(4*M3508底盘电机)+(GM6020yaw电机+电容控制板通信)；CAN2-(GM6020pitch电机+2*M3508摩擦轮电机)+MS4010拨盘电机
 */
void MotorControlCANSend(void)
{

	uint8_t LK_adata_1[8];
	uint8_t LK_adata_2[8];
	uint8_t LK_adata_3[8];
	uint8_t LK_adata_4[8];
	uint8_t LK_adata_5[8];
	uint8_t LK_adata_6[8];
	uint8_t LK_adata_7[8];
	uint8_t LK_adata_8[8];
	static uint16_t send_counter = 0;
	if(0 == send_counter++ % 50)
	{
		LK_errorClear(LK_adata_1);
		LK_errorClear(LK_adata_2);
		LK_errorClear(LK_adata_3);
		LK_errorClear(LK_adata_4);
		LK_errorClear(LK_adata_5);
		LK_errorClear(LK_adata_6);
		LK_errorClear(LK_adata_7);
		LK_errorClear(LK_adata_8);
	}
	else
	{
		LK_iqControl(LK_adata_1, _control->MotorControl.shoulder_target_speed_output[0]);
		CANTransmit_U8(&hcan1, 0x141, LK_adata_1);
		LK_iqControl(LK_adata_2, _control->MotorControl.shoulder_target_speed_output[1]);
		CANTransmit_U8(&hcan1, 0x142, LK_adata_2);
		LK_iqControl(LK_adata_3, _control->MotorControl.elbow_target_speed_output[0]);
		CANTransmit_U8(&hcan1, 0x143, LK_adata_3);	
		LK_iqControl(LK_adata_4, _control->MotorControl.elbow_target_speed_output[1]);
		CANTransmit_U8(&hcan1, 0x144, LK_adata_4);	
		LK_iqControl(LK_adata_5, _control->MotorControl.knee_target_speed_output[0]);
		CANTransmit_U8(&hcan1, 0x145, LK_adata_5);	
		LK_iqControl(LK_adata_6, _control->MotorControl.knee_target_speed_output[1]);
		CANTransmit_U8(&hcan1, 0x146, LK_adata_6);	
		LK_iqControl(LK_adata_7, _control->MotorControl.hip_target_speed_output[0]);
		CANTransmit_U8(&hcan1, 0x147, LK_adata_7);	
		LK_iqControl(LK_adata_8, _control->MotorControl.hip_target_speed_output[1]);
		CANTransmit_U8(&hcan1, 0x148, LK_adata_8);			
	}
}	
 